#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "MPU6050.h"
#include "IMU.h"	//姿态解算
#include "ANO.h"	//匿名上位机驱动
#include "Serial.h" //串口
#include "Canard.h"  //鸭翼
#include "PID.h"
/*
这是我在学习stm32过程中,完成的一个小项目,其中Yaw角不准确的问题,我会在后期更新
建议在此工程的基础上,加入磁力计修正,因时间及其仓促,此工程中不合理之处恐有不少
欢迎联系我解决QQ2576024853   共勉    2024/10/20
*/

/*
修正日志:修正了因MPU6050摆放方向不对导致的Pitch与Roll错误问题
		 修正了Yaw角的旋转不连续性
		 修正了PID失效的问题
*/
int main()
{
	/*模块初始化*/
	OLED_Init();		//OLED初始化
	MPU6050_Init();     //MPU6050初始化
	Serial_Init();		//串口初始化

	MPU6050 MPU;		//定义结构体,用于接收MPU6050原始数据
	Angle angle;		//定义结构体,用于接收欧拉角数据

    while (1) 
		{
		
			MPU6050_GetData(&MPU); //获取MPU6050初始数据,保存到MPU1结构体内
			MPU6050_Kalman(&MPU);	//将MPU1结构体的指针传递到卡尔曼函数并处理
			IMUupdate(&MPU,&angle);//此时结构体内的数据经过了卡尔曼滤波,将其传递到四元数解算函数内,并获取解算的欧拉角
			Canard_Init(&angle);//鸭翼控制初始化(四路舵机)内含PID					
			Canard_ON(&angle);   //鸭翼控制(仅供测试,目前不能用于火箭姿态控制)
			
			ANODT_Send03(angle.Roll*100,angle.Pitch*100,angle.Yaw*100,1);  //向匿名上位机发送欧拉角,参考匿名通信协议数据需要*100,
			
			OLED_ShowString(2, 1, "Pitch:");
			OLED_ShowSignedNum(2, 7, angle.Pitch, 3);					
			OLED_ShowString(4, 1, "Roll:");
			OLED_ShowSignedNum(4, 7, angle.Roll, 3);					
			OLED_ShowString(3, 1, "yaw:");
			OLED_ShowSignedNum(3, 7,angle.Yaw ,3 );				
			OLED_ShowString(1, 1, "IMU System");

		}
}
